/** @file Geometric objects */
#pragma once

#include "util.hpp"
#include "core/error.hpp"

/** A 3D vector */
class Vector
{
public:

  float x,y,z;

  /** Check for nans vector
   * @return if there are nans */
  inline bool hasnans() const
  {
    return isnan(x) || isnan(y) || isnan(z);
  }

  /** Construct */
  Vector() {}

  /** Construct
   * @param X x magnitude
   * @param Y y magnitude
   * @param Z z magnitude */
  Vector(const float X, const float Y, const float Z)
  {
    x = X;
    y = Y;
    z = Z;
    assert(!hasnans());
  }

  inline Vector operator+(const Vector& o) const
  {
    return Vector(x+o.x, y+o.y, z+o.z);
  }

  inline void operator+=(const Vector& o)
  {
    x += o.x;
    y += o.y;
    z += o.z;
    assert(!hasnans());
  }

  inline Vector operator-(const Vector& o) const
  {
    return Vector(x-o.x, y-o.y, z-o.z);
  }

  inline void operator-=(const Vector& o)
  {
    x -= o.x;
    y -= o.y;
    z -= o.z;
    assert(!hasnans());
  }

  inline Vector operator*(const Vector& o) const
  {
    return Vector(x*o.x, y*o.y, z*o.z);
  }

  inline void operator*=(const Vector& o)
  {
    x *= o.x;
    y *= o.y;
    z *= o.z;
    assert(!hasnans());
  }

  inline Vector operator/(const Vector& o) const
  {
    return Vector(x/o.x, y/o.y, z/o.z);
  }

  inline void operator/=(const Vector& o)
  {
    x /= o.x;
    y /= o.y;
    z /= o.z;
  }

  inline Vector operator*(const float o) const
  {
    return Vector(x*o, y*o, z*o);
  }

  inline void operator*=(const float o)
  {
    x *= o;
    y *= o;
    z *= o;
    assert(!hasnans());
  }
 
  inline Vector operator/(const float o) const
  {
    const float inv = 1.f/o;
    return Vector(x*inv, y*inv, z*inv);
  }

  inline void operator/=(const float o)
  {
    const float inv = 1.f/o;
    x *= inv;
    y *= inv;
    z *= inv;
    assert(!hasnans());
  }

  /** Get the length of the vector
   * @return length */
  float length() const
  {
    return sqrtf(x*x + y*y + z*z);
  }

  inline Vector normalized() const
  {
    return (*this)/this->length();
  }

  inline void normalize()
  {
    *this /= this->length();
  }

  inline bool operator==(const Vector& o) const
  {
    return x==o.x && y==o.y && z==o.z;
  }

  inline bool operator!=(const Vector& o) const
  {
    return !((*this) == o);
  }
};

inline float dot(const Vector& a, const Vector& b)
{
  return a.x*b.x + a.y*b.y + a.z*b.z;
}

inline Vector cross(const Vector& a, const Vector& b)
{
  return Vector(a.y*b.z - b.y*a.z,
		-(a.x*b.x - b.x*a.z),
		a.x*b.y - b.x*a.y);
}

inline Vector operator*(const float a, const Vector& b)
{
  return b*a;
}

/** A 3D point */
class Point
{
public:

  float x,y,z;

  /** Check for nans point
   * @return if there are nans */
  inline bool hasnans() const
  {
    return isnan(x) || isnan(y) || isnan(z);
  }

  /** Construct */
  Point() {}

  /** Construct
   * @param X x magnitude
   * @param Y y magnitude
   * @param Z z magnitude */
  Point(const float X, const float Y, const float Z)
  {
    x = X;
    y = Y;
    z = Z;
    assert(!hasnans());
  }

  inline Point operator+(const Point& o) const
  {
    return Point(x+o.x, y+o.y, z+o.z);
  }

  inline void operator+=(const Point& o)
  {
    x += o.x;
    y += o.y;
    z += o.z;
    assert(!hasnans());
  }

  inline Vector operator-(const Point& o) const
  {
    return Vector(x-o.x, y-o.y, z-o.z);
  }

  inline Point operator+(const Vector& o) const
  {
    return Point(x+o.x, y+o.y, z+o.z);
  }

  inline void operator+=(const Vector& o)
  {
    x += o.x;
    y += o.y;
    z += o.z;
    assert(!hasnans());
  }

  Point operator/(const float o) const
  {
    const float inv = 1.f/o;
    return Point(x*inv, y*inv, z*inv);
  }

  inline void operator/=(const float o)
  {
    const float inv = 1.f/o;
    x *= inv;
    y *= inv;
    z *= inv;
    assert(!hasnans());
  }

  /** Get the length of the vector
   * @return length */
  float length() const
  {
    return sqrtf(x*x + y*y + z*z);
  }

  inline Point normalized() const
  {
    return (*this)/this->length();
  }

  inline void normalize()
  {
    *this /= this->length();
  }

  inline bool operator==(const Point& o) const
  {
    return x==o.x && y==o.y && z==o.z;
  }

  inline bool operator!=(const Point& o) const
  {
    return !((*this) == o);
  }
};



/** A 3D normal */
class Normal
{
public:

  float x,y,z;

  /** Check for nans normal
   * @return if there are nans */
  inline bool hasnans() const
  {
    return isnan(x) || isnan(y) || isnan(z);
  }

  /** Construct */
  Normal() {}

  /** Construct
   * @param X x magnitude
   * @param Y y magnitude
   * @param Z z magnitude */
  Normal(const float X, const float Y, const float Z)
  {
    x = X;
    y = Y;
    z = Z;
    assert(!hasnans());
  }

  explicit Normal(const Vector& o)
  {
    x = o.x;
    y = o.y;
    z = o.z;
    assert(!hasnans());
  }

  explicit operator Vector() const
  {
    return Vector(x,y,z);
  }

  inline Normal operator+(const Normal& o) const
  {
    return Normal(x+o.x, y+o.y, z+o.z);
  }

  inline void operator+=(const Normal& o)
  {
    x += o.x;
    y += o.y;
    z += o.z;
    assert(!hasnans());
  }

  inline Normal operator-(const Normal& o) const
  {
    return Normal(x-o.x, y-o.y, z-o.z);
  }

  inline void operator-=(const Normal& o)
  {
    x -= o.x;
    y -= o.y;
    z -= o.z;
    assert(!hasnans());
  }

  inline Normal operator*(const Normal& o) const
  {
    return Normal(x*o.x, y*o.y, z*o.z);
  }

  inline void operator*=(const Normal& o)
  {
    x *= o.x;
    y *= o.y;
    z *= o.z;
    assert(!hasnans());
  }

  inline Normal operator/(const Normal& o) const
  {
    return Normal(x/o.x, y/o.y, z/o.z);
  }

  inline void operator/=(const Normal& o)
  {
    x /= o.x;
    y /= o.y;
    z /= o.z;
  }

  inline Normal operator*(const float o) const
  {
    return Normal(x*o, y*o, z*o);
  }

  inline void operator*=(const float o)
  {
    x *= o;
    y *= o;
    z *= o;
    assert(!hasnans());
  }
 
  inline Normal operator/(const float o) const
  {
    const float inv = 1.f/o;
    return Normal(x*inv, y*inv, z*inv);
  }

  inline void operator/=(const float o)
  {
    const float inv = 1.f/o;
    x *= inv;
    y *= inv;
    z *= inv;
    assert(!hasnans());
  }

  /** Get the length of the vector
   * @return length */
  float length() const
  {
    return sqrtf(x*x + y*y + z*z);
  }

  inline Normal normalized() const
  {
    return (*this)/this->length();
  }

  inline void normalize()
  {
    *this /= this->length();
  }

  inline bool operator==(const Normal& o) const
  {
    return x==o.x && y==o.y && z==o.z;
  }

  inline bool operator!=(const Normal& o) const
  {
    return !((*this) == o);
  }
};

inline float dot(const Normal& a, const Normal& b)
{
  return a.x*b.x + a.y*b.y + a.z*b.z;
}

inline Normal cross(const Normal& a, const Normal& b)
{
  return Normal(a.y*b.z - b.y*a.z,
		-(a.x*b.x - b.x*a.z),
		a.x*b.y - b.x*a.y);
}

inline Normal operator*(const float a, const Normal& b)
{
  return b*a;
}

/** A ray
 * This consits of an origin and a direction */
class Ray
{
public:

  /** The origin */
  Point o;
  /** The direction */
  Vector d;
  
  /** Check for nans ray
   * @return of there are nans */
  inline bool hasnans() const
  {
    return o.hasnans() || d.hasnans();
  }

  /** Construct */
  Ray() {}

  /** Construct
   * @param o origin
   * @param d direction */
  Ray(const Point& O, const Vector& D) : o(O), d(D)
  {
    assert(!hasnans());
  }

  /** Project float along ray
   * @param o float to project
   * @return location along ray */
  inline Point operator()(const float f) const
  {
    return o + d*f;
  }
};

